/*
 * Copyright (c) 2022 HiSilicon (Shanghai) Technologies CO., LIMITED.
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/* -------------------------------------------------------------------------- */
/* --- PUBLIC DEPENDANCIES -------------------------------------------------- */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <pthread.h>
#include <sys/prctl.h>
#include <math.h>

extern "C" {
#include "hi_common.h"
#include "hi_comm_sys.h"
#include "hi_comm_svp.h"
#include "sample_comm.h"

/* -------------------------------------------------------------------------- */
/* --- COMMON DEPENDANCIES -------------------------------------------------- */
#include "sample_comm_svp.h"
#include "sample_comm_nnie.h"
#include "sample_comm_ive.h"

/* -------------------------------------------------------------------------- */
/* --- MPP DEPENDANCIES ----------------------------------------------------- */
#include "ive_img.h"
#include "vgs_img.h"

/* -------------------------------------------------------------------------- */
/* --- PRIVATE DEPENDANCIES ------------------------------------------------- */
#include "uart.h"
#include "trace.h"
#include "lbph_tracking.h"
#include "svp_nnie_application.h"
#include "main_process.h"
}

#include "kalman_tracking.hpp"

/* -------------------------------------------------------------------------- */
/* --- PRIVATE VARIABLES ---------------------------------------------------- */
static IVE_IMAGE_S img;
static HI_BOOL g_stop_signal = HI_FALSE;
static SAMPLE_SVP_NNIE_MODEL_S s_stYolov5Model;
static SAMPLE_SVP_NNIE_PARAM_S s_stYolov5NnieParam;

static HI_U8 TrackGestureObjNum = 0;
static HI_U8 TrackFaceObjNum = 0;
static RectBox TrackGestureBoxs[SAMPLE_IVE_KCF_MAX_BBOX];
static RectBox TrackFaceBoxs[SAMPLE_IVE_KCF_MAX_BBOX];
static KalmanTracking tracker;

/* Yolov5 lite params setting */
static HI_FLOAT strides[3] = {8., 16., 32.};
static HI_U32 outputMapSize[3] = {40, 20, 10};

/* for 416 */
static ANCHOR_W_H anchor_grids[3][3] = {
    {{10.0f, 13.0f}, {16.0f, 30.0f}, {33.0f, 23.0f}},
    {{30.0f, 61.0f}, {62.0f, 45.0f}, {59.0f, 119.0f}},
    {{116.0f, 90.0f}, {156.0f, 198.0f}, {373.0f, 326.0f}}};

/* for 320 */
// static ANCHOR_W_H anchor_grids[3][3] = {
//     {{8.7890625f, 10.1953125f}, {24.78125f, 15.4765625f}, {29.734375f, 41.15625f}},
//     {{63.5625f, 42.625f}, {49.9375f, 85.6875f}, {69.0f, 147.0f}},
//     {{143.625f, 107.625f}, {182.875f, 195.5f}, {314.25f, 209.75f}}};

extern int uartFd;
/* -------------------------------------------------------------------------- */
/* --- PRIVATE FUNCTION (GLOBAL) -------------------------------------------- */
/* function : yolo process singalHandle */
void SVP_NNIE_Yolov5_HandleSig( void )
{
    g_stop_signal = HI_TRUE;
}

/* function : Yolov5 Deinit */
static HI_S32 SVP_NNIE_Yolov5_Deinit(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam, SAMPLE_SVP_NNIE_MODEL_S *pstNnieModel)
{
    HI_S32 s32Ret = HI_SUCCESS;
    /* hardware deinit */
    if (pstNnieParam != NULL) {
        s32Ret = SAMPLE_COMM_SVP_NNIE_ParamDeinit(pstNnieParam);
        SAMPLE_SVP_CHECK_EXPR_TRACE(s32Ret != HI_SUCCESS, SAMPLE_SVP_ERR_LEVEL_ERROR,
            "Error,SAMPLE_COMM_SVP_NNIE_ParamDeinit failed!\n");
    }
    /* model deinit */
    if (pstNnieModel != NULL) {
        s32Ret = SAMPLE_COMM_SVP_NNIE_UnloadModel(pstNnieModel);
        SAMPLE_SVP_CHECK_EXPR_TRACE(s32Ret != HI_SUCCESS, SAMPLE_SVP_ERR_LEVEL_ERROR,
            "Error,SAMPLE_COMM_SVP_NNIE_UnloadModel failed!\n");
    }
    return s32Ret;
}

/* function : Yolov5 Stop */
static hi_void SVP_NNIE_Yolov5_Lite_Stop(hi_void)
{
    SVP_NNIE_Yolov5_Deinit(&s_stYolov5NnieParam, &s_stYolov5Model);
    (HI_VOID)memset_s(&s_stYolov5NnieParam, sizeof(SAMPLE_SVP_NNIE_PARAM_S), 0, sizeof(SAMPLE_SVP_NNIE_PARAM_S));
    (HI_VOID)memset_s(&s_stYolov5Model, sizeof(SAMPLE_SVP_NNIE_MODEL_S), 0, sizeof(SAMPLE_SVP_NNIE_MODEL_S));
    SAMPLE_COMM_SVP_CheckSysExit();
    printf("\033[0;31mprogram termination abnormally!\033[0;39m\n");
}

/* function : Yolov3 init */
static HI_S32 SVP_NNIE_Yolov5_ParamInit(SAMPLE_SVP_NNIE_CFG_S *pstCfg, SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam)
{
    HI_S32 s32Ret = HI_SUCCESS;
    /* init hardware para */
    s32Ret = SAMPLE_COMM_SVP_NNIE_ParamInit(pstCfg, pstNnieParam);
    if (s32Ret != HI_SUCCESS) {
        SAMPLE_PRT("Error(%#x),SAMPLE_COMM_SVP_NNIE_ParamInit failed!\n", s32Ret);
    }
    return s32Ret;
}

/* function : Fill Src Data */
static HI_S32 SAMPLE_SVP_NNIE_FillSrcData(SAMPLE_SVP_NNIE_CFG_S *pstNnieCfg, SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam,
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S *pstInputDataIdx)
{
    FILE *fp = NULL;
    HI_U32 i = 0, j = 0, n = 0;
    HI_U32 u32Height = 0, u32Width = 0, u32Chn = 0, u32Stride = 0, u32Dim = 0;
    HI_U32 u32VarSize = 0;
    HI_U8 *pu8PicAddr = NULL;
    HI_U32 *pu32StepAddr = NULL;
    HI_U32 u32SegIdx = pstInputDataIdx->u32SegIdx;
    HI_U32 u32NodeIdx = pstInputDataIdx->u32NodeIdx;
    HI_U32 u32TotalStepNum = 0;
    HI_ULONG ulSize;
    HI_CHAR path[PATH_MAX] = {0};

    /* open file */
    SAMPLE_SVP_CHECK_EXPR_RET(pstNnieCfg->pszPic == HI_NULL, HI_INVALID_VALUE, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error, pstNnieCfg->pszPic is null!\n");
    SAMPLE_SVP_CHECK_EXPR_RET(pstInputDataIdx->u32SegIdx >= SVP_NNIE_MAX_NET_SEG_NUM, HI_INVALID_VALUE,
        SAMPLE_SVP_ERR_LEVEL_ERROR, "Error, u32SegIdx should be less than %u!\n", SVP_NNIE_MAX_NET_SEG_NUM);
    SAMPLE_SVP_CHECK_EXPR_RET(pstInputDataIdx->u32NodeIdx >= SVP_NNIE_MAX_INPUT_NUM, HI_INVALID_VALUE,
        SAMPLE_SVP_ERR_LEVEL_ERROR, "Error, u32NodeIdx should be less than %u!\n", SVP_NNIE_MAX_INPUT_NUM);

    SAMPLE_SVP_CHECK_EXPR_RET((strlen(pstNnieCfg->pszPic) > PATH_MAX) ||
        (realpath(pstNnieCfg->pszPic, path) == HI_NULL),
        HI_ERR_SVP_NNIE_ILLEGAL_PARAM, SAMPLE_SVP_ERR_LEVEL_ERROR, "Error, file_name is invalid!\n");
    fp = fopen(path, "rb");
    SAMPLE_SVP_CHECK_EXPR_RET(fp == NULL, HI_INVALID_VALUE, SAMPLE_SVP_ERR_LEVEL_ERROR, "Error, open file failed!\n");

    /* get data size */
    if (SVP_BLOB_TYPE_U8 <= pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].enType &&
        SVP_BLOB_TYPE_YVU422SP >= pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].enType) {
        u32VarSize = sizeof(HI_U8);
    } else {
        u32VarSize = sizeof(HI_U32);
    }

    /* fill src data */
    if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].enType) {
        u32Dim = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].unShape.stSeq.u32Dim;
        u32Stride = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Stride;
        pu32StepAddr = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].unShape.stSeq.u64VirAddrStep);
        pu8PicAddr = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U8,
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64VirAddr);
        for (n = 0; n < pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Num; n++) {
            for (i = 0; i < *(pu32StepAddr + n); i++) {
                ulSize = fread(pu8PicAddr, u32Dim * u32VarSize, 1, fp);
                SAMPLE_SVP_CHECK_EXPR_GOTO(ulSize != 1, FAIL, SAMPLE_SVP_ERR_LEVEL_ERROR,
                    "Error,Read image file failed!\n");
                pu8PicAddr += u32Stride;
            }
            u32TotalStepNum += *(pu32StepAddr + n);
        }
        SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64PhyAddr,
            SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64VirAddr),
            u32TotalStepNum * u32Stride);
    } else {
        u32Height = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].unShape.stWhc.u32Height;
        u32Width = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].unShape.stWhc.u32Width;
        u32Chn = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].unShape.stWhc.u32Chn;
        u32Stride = pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Stride;
        pu8PicAddr = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U8,
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64VirAddr);
        if (SVP_BLOB_TYPE_YVU420SP == pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].enType) {
            for (n = 0; n < pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Num; n++) {
                for (i = 0; i < u32Chn * u32Height / 2; i++) { /* Brightness: 1 height, Chroma: 1/2 height */
                    ulSize = fread(pu8PicAddr, u32Width * u32VarSize, 1, fp);
                    SAMPLE_SVP_CHECK_EXPR_GOTO(ulSize != 1, FAIL, SAMPLE_SVP_ERR_LEVEL_ERROR,
                        "Error,Read image file failed!\n");
                    pu8PicAddr += u32Stride;
                }
            }
        } else if (SVP_BLOB_TYPE_YVU422SP == pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].enType) {
            for (n = 0; n < pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Num; n++) {
                for (i = 0; i < u32Height * 2; i++) { /* Brightness: 1 height, Chroma: 1 height */
                    ulSize = fread(pu8PicAddr, u32Width * u32VarSize, 1, fp);
                    SAMPLE_SVP_CHECK_EXPR_GOTO(ulSize != 1, FAIL, SAMPLE_SVP_ERR_LEVEL_ERROR,
                        "Error,Read image file failed!\n");
                    pu8PicAddr += u32Stride;
                }
            }
        } else {
            for (n = 0; n < pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Num; n++) {
                for (i = 0; i < u32Chn; i++) {
                    for (j = 0; j < u32Height; j++) {
                        ulSize = fread(pu8PicAddr, u32Width * u32VarSize, 1, fp);
                        SAMPLE_SVP_CHECK_EXPR_GOTO(ulSize != 1, FAIL, SAMPLE_SVP_ERR_LEVEL_ERROR,
                            "Error,Read image file failed!\n");
                        pu8PicAddr += u32Stride;
                    }
                }
            }
        }
        SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64PhyAddr,
            SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u64VirAddr),
            pstNnieParam->astSegData[u32SegIdx].astSrc[u32NodeIdx].u32Num * u32Chn * u32Height * u32Stride);
    }

    (HI_VOID)fclose(fp);
    return HI_SUCCESS;
FAIL:

    (HI_VOID)fclose(fp);
    return HI_FAILURE;
}

/* function : NNIE Forward */
static HI_S32 SAMPLE_SVP_NNIE_Forward(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam,
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S *pstInputDataIdx, SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S *pstProcSegIdx,
    HI_BOOL bInstant)
{
    HI_S32 s32Ret = HI_SUCCESS;
    HI_U32 i, j;
    HI_BOOL bFinish = HI_FALSE;
    SVP_NNIE_HANDLE hSvpNnieHandle = 0;
    HI_U32 u32TotalStepNum = 0;

    SAMPLE_SVP_CHECK_EXPR_RET(pstProcSegIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstInputDataIdx->u32SegIdx >= pstNnieParam->pstModel->u32NetSegNum ||
        pstNnieParam->pstModel->u32NetSegNum > SVP_NNIE_MAX_NET_SEG_NUM,
        HI_INVALID_VALUE, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error, pstProcSegIdx->u32SegIdx(%u) and pstInputDataIdx->u32SegIdx(%u) "
        "should be less than %u, pstNnieParam->pstModel->u32NetSegNum(%u) can't be greater than %u!\n",
        pstProcSegIdx->u32SegIdx, pstInputDataIdx->u32SegIdx, pstNnieParam->pstModel->u32NetSegNum,
        pstNnieParam->pstModel->u32NetSegNum, SVP_NNIE_MAX_NET_SEG_NUM);

    SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64PhyAddr,
        SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
        pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u64VirAddr),
        pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].stTskBuf.u32Size);

    for (i = 0; i < pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType == SVP_BLOB_TYPE_SEQ_S32) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    /* set input blob according to node name */
    if (pstInputDataIdx->u32SegIdx != pstProcSegIdx->u32SegIdx) {
        for (i = 0; i < pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].u16SrcNum; i++) {
            for (j = 0; j < pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum; j++) {
                if (strncmp(pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].astDstNode[j].szName,
                    pstNnieParam->pstModel->astSeg[pstProcSegIdx->u32SegIdx].astSrcNode[i].szName,
                    SVP_NNIE_NODE_NAME_LEN) == 0) {
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc[i] =
                        pstNnieParam->astSegData[pstInputDataIdx->u32SegIdx].astDst[j];
                    break;
                }
            }
            SAMPLE_SVP_CHECK_EXPR_RET((j == pstNnieParam->pstModel->astSeg[pstInputDataIdx->u32SegIdx].u16DstNum),
                HI_FAILURE, SAMPLE_SVP_ERR_LEVEL_ERROR, "Error,can't find %d-th seg's %d-th src blob!\n",
                pstProcSegIdx->u32SegIdx, i);
        }
    }

    /* NNIE_Forward */
    s32Ret = HI_MPI_SVP_NNIE_Forward(&hSvpNnieHandle, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astSrc,
        pstNnieParam->pstModel, pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst,
        &pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx], bInstant);
    SAMPLE_SVP_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_Forward failed!\n");

    if (bInstant) {
        /* Wait NNIE finish */
        while (HI_ERR_SVP_NNIE_QUERY_TIMEOUT == (s32Ret = HI_MPI_SVP_NNIE_Query(
            pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].enNnieId, hSvpNnieHandle, &bFinish, HI_TRUE))) {
            usleep(100); /* sleep 100 micro_seconds */
            SAMPLE_SVP_TRACE(SAMPLE_SVP_ERR_LEVEL_INFO, "HI_MPI_SVP_NNIE_Query Query timeout!\n");
        }
    }
    u32TotalStepNum = 0;
    for (i = 0; i < pstNnieParam->astForwardCtrl[pstProcSegIdx->u32SegIdx].u32DstNum; i++) {
        if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].enType) {
            for (j = 0; j < pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num; j++) {
                u32TotalStepNum += *(SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U32,
                    pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stSeq.u64VirAddrStep) +
                    j);
            }
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                u32TotalStepNum * pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        } else {
            SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64PhyAddr,
                SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID,
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u64VirAddr),
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Num *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Chn *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].unShape.stWhc.u32Height *
                pstNnieParam->astSegData[pstProcSegIdx->u32SegIdx].astDst[i].u32Stride);
        }
    }

    return s32Ret;
}

/* function : deal with caculate result from .wk model. */
static HI_S32 SVP_NNIE_Yolov5_GetResult(SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam, SVP_NNIE_YOLOV5_BBOX_S **result_node)
{
    HI_S32 s32Ret = HI_SUCCESS;
    HI_U8 layer_index;
    HI_U32 ch, class_index, coord_xy, index_x, index_y;

    /* Output result addr */
    HI_S32 *ps32InputBlob = NULL;

    /* Output result shape 3 * 85 * 1600 400 100*/ 
    HI_U16 u16Chn;
    HI_U16 u16ClassNum;
    HI_U16 u16Size;
    HI_FLOAT anchor_w; 
    HI_FLOAT anchor_h; 
    HI_FLOAT confidence;
    HI_FLOAT class_confidence;
    HI_FLOAT iou_x;
    HI_FLOAT iou_y;
    HI_FLOAT iou_w;
    HI_FLOAT iou_h;

    /* Output list node define */
    SVP_NNIE_YOLOV5_BBOX_S *cur_node = NULL;
    SVP_NNIE_YOLOV5_BBOX_S *pre_node = NULL;
    *result_node = NULL;

    for (layer_index = 0; layer_index < SVP_NNIE_YOLOV5_RESULT_OUTPUT_NUM; layer_index++) {
        u16Chn      = pstNnieParam->astSegData[0].astDst[layer_index + OFFSETIDX].unShape.stWhc.u32Chn;       // 3
        u16ClassNum = pstNnieParam->astSegData[0].astDst[layer_index + OFFSETIDX].unShape.stWhc.u32Height;    // 85
        u16Size     = pstNnieParam->astSegData[0].astDst[layer_index + OFFSETIDX].unShape.stWhc.u32Width;     // 40*40 20*20 10*10
        // MSG_DEBUG(DEBUG_DETECTION, "output size:  %d, %d, %d\n", u16Chn, u16ClassNum, u16Size);
        
        /* Deal with unfinished result */
        ps32InputBlob = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_S32, pstNnieParam->astSegData[0].astDst[layer_index + OFFSETIDX].u64VirAddr);
        
        /* Deal with result method: direct take the data from addr, don't need to permute */
        for (ch = 0; ch < u16Chn; ch++) {
            
            /* Take the anchor_grids */
            anchor_w = anchor_grids[layer_index][ch].anchor_w;
            anchor_h = anchor_grids[layer_index][ch].anchor_h;

            /* Traversal all single data buf */
            for (coord_xy = 0; coord_xy < u16Size; coord_xy++) {

                /* Caculate the strides */
                index_y = coord_xy / outputMapSize[layer_index];
                index_x = coord_xy % outputMapSize[layer_index];

                /* Caculate this iou confidence, this data index is 4 */
                confidence = *(ps32InputBlob + ch * u16Size * u16ClassNum + 4 * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                confidence = SAMPLE_SVP_NNIE_SIGMOID(confidence);

                /* If confidence valid, judge the class confidence if vaild */
                if (confidence > CONF_THRESH) {

                    for (class_index = 5; class_index < u16ClassNum; class_index++) {
                        class_confidence = *(ps32InputBlob + ch * u16Size * u16ClassNum + class_index * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                        class_confidence = SAMPLE_SVP_NNIE_SIGMOID(class_confidence) * confidence;

                        /* If class confidence vaild, caculate the iou coord */
                        if (class_confidence > CONF_THRESH) {

                            /* Take the (x y w h) in index 0 1 2 3 */
                            iou_x = *(ps32InputBlob + ch * u16Size * u16ClassNum + 0 * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                            iou_y = *(ps32InputBlob + ch * u16Size * u16ClassNum + 1 * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                            iou_w = *(ps32InputBlob + ch * u16Size * u16ClassNum + 2 * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                            iou_h = *(ps32InputBlob + ch * u16Size * u16ClassNum + 3 * u16Size + coord_xy) / SAMPLE_SVP_NNIE_QUANT_BASE;
                            
                            /* Post process: sigmod -> mul sub add mul / mul pow mul -> result */
                            iou_x = SAMPLE_SVP_NNIE_SIGMOID(iou_x);
                            iou_y = SAMPLE_SVP_NNIE_SIGMOID(iou_y);
                            iou_w = SAMPLE_SVP_NNIE_SIGMOID(iou_w);
                            iou_h = SAMPLE_SVP_NNIE_SIGMOID(iou_h);

                            iou_x = (iou_x * 2.0f - 0.5f + (HI_FLOAT)index_x) * strides[layer_index];
                            iou_y = (iou_y * 2.0f - 0.5f + (HI_FLOAT)index_y) * strides[layer_index];
                            iou_w = (iou_w * 2.0f) * (iou_w * 2.0f) * anchor_w;
                            iou_h = (iou_h * 2.0f) * (iou_h * 2.0f) * anchor_h;

                            /* Use result to record the vaild bbox */
                            cur_node = (SVP_NNIE_YOLOV5_BBOX_S *) malloc(sizeof( SVP_NNIE_YOLOV5_BBOX_S ));

                            /* Get the bbox coord, class type and confidence */                            
                            cur_node->bBox.xmin = SAMPLE_SVP_NNIE_MIN(SAMPLE_SVP_NNIE_MAX((iou_x - 0.5f * iou_w), 0.0f), YOLO_MODEL_WIDTH);
                            cur_node->bBox.ymin = SAMPLE_SVP_NNIE_MIN(SAMPLE_SVP_NNIE_MAX((iou_y - 0.5f * iou_h), 0.0f), YOLO_MODEL_HEIGHT);
                            cur_node->bBox.xmax = SAMPLE_SVP_NNIE_MAX(SAMPLE_SVP_NNIE_MIN((iou_x + 0.5f * iou_w), YOLO_MODEL_WIDTH), 0);
                            cur_node->bBox.ymax = SAMPLE_SVP_NNIE_MAX(SAMPLE_SVP_NNIE_MIN((iou_y + 0.5f * iou_h), YOLO_MODEL_HEIGHT), 0);
                            cur_node->u8ClassIdx = class_index - 5;
                            cur_node->f32ClsScore = class_confidence;

                            cur_node->next = NULL;

                            /* Move to next list node */
                            if (*result_node == NULL) *result_node = cur_node;
                            else pre_node->next = cur_node;
                            pre_node = cur_node;
                        }
                    }
                }
            }
        }
    }
    return s32Ret;
}

/* 
* 对链表进行归并排序 O（nlog n） function include ( toSortList merge )
* Merge sort on a linked list O(nlog n)
*/
static SVP_NNIE_YOLOV5_BBOX_S* merge(SVP_NNIE_YOLOV5_BBOX_S* head1, SVP_NNIE_YOLOV5_BBOX_S* head2, SVP_NNIE_SORT_MODE_S mode, HI_FLOAT cx)
{
    SVP_NNIE_YOLOV5_BBOX_S* dummyHead = (SVP_NNIE_YOLOV5_BBOX_S*)malloc(sizeof(SVP_NNIE_YOLOV5_BBOX_S));
    SVP_NNIE_YOLOV5_BBOX_S *temp = dummyHead, *temp1 = head1, *temp2 = head2;
    while (temp1 != NULL && temp2 != NULL)
    {
        if (mode == SORT_SCORE_MODE)
        {
            if (temp1->f32ClsScore > temp2->f32ClsScore)
            {
                temp->next = temp1;
                temp1 = temp1->next;
            } else {
                temp->next = temp2;
                temp2 = temp2->next;
            }
        } else {
            float dx_1 = fabsf((temp1->bBox.xmax + temp1->bBox.xmin) / 2. - cx);
            float dx_2 = fabsf((temp2->bBox.xmax + temp2->bBox.xmin) / 2. - cx);
            if (dx_1 <= dx_2)
            {
                temp->next = temp1;
                temp1 = temp1->next;
            } else {
                temp->next = temp2;
                temp2 = temp2->next;
            }
        }
        temp = temp->next;
    }
    if (temp1 != NULL) 
    {
        temp->next = temp1;
    } else if (temp2 != NULL) {
        temp->next = temp2;
    }
    return dummyHead->next;
}

static SVP_NNIE_YOLOV5_BBOX_S* toSortList(SVP_NNIE_YOLOV5_BBOX_S* head, SVP_NNIE_YOLOV5_BBOX_S* tail, SVP_NNIE_SORT_MODE_S mode, HI_FLOAT cx)
{
    if (head == NULL) return head;
    if (head->next == tail)
    {
        head->next = NULL;
        return head;
    }

    SVP_NNIE_YOLOV5_BBOX_S *slow = head, *fast = head;
    while (fast != tail)
    {
        slow = slow->next;
        fast = fast->next;
        if (fast != tail) fast = fast->next;
    }
    SVP_NNIE_YOLOV5_BBOX_S* mid = slow;
    return merge(toSortList(head, mid, mode, cx), toSortList(mid, tail, mode, cx), mode, cx);
}

/* function : printf the result */
static void SAMPLE_SVP_NNIE_Detection_PrintResult(SVP_NNIE_YOLOV5_BBOX_S *result_node)
{
#if DEBUG_DETECTION
    if (result_node != NULL) {
        MSG_DEBUG(DEBUG_DETECTION, ">>>>>>>>>>>>>>Detection Result>>>>>>>>>>>>>>>\n");
        while (result_node != NULL) {
            MSG_DEBUG(DEBUG_DETECTION, "Detection iou: [ %.2f, %.2f, %.2f, %.2f ], Class: %d, Score: %f, cx : %.2f\n", 
                        result_node->bBox.xmin, result_node->bBox.xmax,
                        result_node->bBox.ymin, result_node->bBox.ymax,
                        result_node->u8ClassIdx, result_node->f32ClsScore,
                        fabsf((result_node->bBox.xmax + result_node->bBox.xmin) / 2. - YOLO_MODEL_WIDTH / 2));
            result_node = result_node->next;
        }
        MSG_DEBUG(DEBUG_DETECTION, ">>>>>>>>>>>>>>Show Result end>>>>>>>>>>>>>>>>\n");
    }
#endif
}

/* function : nms */
static void SVP_NNIE_Yolov5_NonMaxSuppression(SVP_NNIE_YOLOV5_BBOX_S *result_node)
{
    /* Yolov5 NonMaxSuppression operation */
	SVP_NNIE_YOLOV5_BBOX_S *Cmp_Bbox2 = NULL;
	SVP_NNIE_YOLOV5_BBOX_S *Pre_Cmp_Bbox2 = NULL;
	SVP_NNIE_YOLOV5_BBOX_S *Cur_Bbox = result_node;
	SVP_NNIE_YOLOV5_BBOX_S *Temp_Bbox = NULL;

    HI_FLOAT f32InterWidth = 0.0f;
    HI_FLOAT f32InterHeight = 0.0f;

    HI_DOUBLE f64InterArea = 0.0f;
    HI_DOUBLE f64Box1Area = 0.0f;
    HI_DOUBLE f64Box2Area = 0.0f;

    HI_DOUBLE nms_ratio = 0.0f;

	while (Cur_Bbox != NULL) {
	
		Cmp_Bbox2 = Cur_Bbox->next;
		Pre_Cmp_Bbox2 = Cur_Bbox;

        f64Box1Area = (Cur_Bbox->bBox.xmax - Cur_Bbox->bBox.xmin) * (Cur_Bbox->bBox.ymax - Cur_Bbox->bBox.ymin);
		while (Cmp_Bbox2 != NULL) {
			if (Cmp_Bbox2->u8ClassIdx != Cur_Bbox->u8ClassIdx) {
                Pre_Cmp_Bbox2 = Cmp_Bbox2;
                Cmp_Bbox2 = Cmp_Bbox2->next;
                continue;
            }

            f64Box2Area = (Cmp_Bbox2->bBox.xmax - Cmp_Bbox2->bBox.xmin) * (Cmp_Bbox2->bBox.ymax - Cmp_Bbox2->bBox.ymin); 
            
            /* Caculate the overlap area size */
            f32InterWidth   = SAMPLE_SVP_NNIE_MIN(Cur_Bbox->bBox.xmax, Cmp_Bbox2->bBox.xmax) -
                                SAMPLE_SVP_NNIE_MAX(Cur_Bbox->bBox.xmin, Cmp_Bbox2->bBox.xmin);
            f32InterHeight  = SAMPLE_SVP_NNIE_MIN(Cur_Bbox->bBox.ymax, Cmp_Bbox2->bBox.ymax) -
                                SAMPLE_SVP_NNIE_MAX(Cur_Bbox->bBox.ymin, Cmp_Bbox2->bBox.ymin);
			f64InterArea = SAMPLE_SVP_NNIE_MAX((f32InterWidth * f32InterHeight), 0.0f);
            
            if (f32InterWidth <= 0 || f32InterHeight <= 0) {
                f64InterArea = 0;
            }

			nms_ratio = f64InterArea / (f64Box1Area + f64Box2Area - f64InterArea);
			
            /* Judge the nms ratio if bigger than thresh, rebuild the list node */
			if (nms_ratio > NMS_THRESH) {
				Temp_Bbox = Cmp_Bbox2;
				Cmp_Bbox2 = Cmp_Bbox2->next;
				Pre_Cmp_Bbox2->next = Cmp_Bbox2;
				free(Temp_Bbox);
			} else {
				Pre_Cmp_Bbox2 = Cmp_Bbox2;
				Cmp_Bbox2 = Cmp_Bbox2->next;
			}
		}
		Cur_Bbox = Cur_Bbox->next;
	}
}

/* function : draw result rect in dst frm */
static HI_S32 SAMPLE_SVP_NNIE_DrawResultRect(SVP_NNIE_YOLOV5_BBOX_S *node, VIDEO_FRAME_INFO_S *dst)
{
    HI_S32 s32Ret = HI_SUCCESS;
#if DEBUG_DRAWRECT
    RectBox bBox = { 0 };
    while (node != NULL) {
        if (node->u8ClassIdx > 0)
        {
            bBox = node->bBox;
            RectBoxTran(&bBox, YOLO_MODEL_WIDTH, YOLO_MODEL_HEIGHT, dst->stVFrame.u32Width, dst->stVFrame.u32Height);
            s32Ret = MppFrmDrawRects(dst, &bBox, 1, RGB888_RED, DRAW_RETC_THICK);
        }
        node = node->next;
    }
#endif
    return s32Ret;
}

/* function : release the result */
static void SAMPLE_SVP_NNIE_Release_Result(SVP_NNIE_YOLOV5_BBOX_S *result_node)
{
	SVP_NNIE_YOLOV5_BBOX_S *temp = NULL;
	while (result_node != NULL) {
		temp = result_node;
		result_node = result_node->next;
		free(temp);
        temp = NULL;
	}
}

/* function : show YOLOV5 sample(image 416x416 U8_C3) */
void SVP_NNIE_Yolov5_Lite( void )
{
    /* Setting the .wk model and detect image path. */
    const HI_CHAR *pcSrcFile = "./data/image/dog_bike_car_416x416.bgr";
    const HI_CHAR *pcModelName = "./data/model/yolov5_416u8_high_rgb_inst.wk";
    const HI_U32 u32PicNum = 1;

    MSG_DEBUG(DEBUG_DETECTION, "Load model: %s\n", pcModelName);

    HI_S32 s32Ret = HI_SUCCESS;
    SAMPLE_SVP_NNIE_CFG_S stNnieCfg = { 0 };
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S stInputDataIdx = { 0 };
    SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S stProcSegIdx = { 0 };
    SVP_NNIE_YOLOV5_BBOX_S *output_node = NULL;

    /* Set configuration parameter */
    stNnieCfg.pszPic = pcSrcFile;
    stNnieCfg.u32MaxInputNum = u32PicNum; // max input image num in each batch
    stNnieCfg.u32MaxRoiNum = 0;
    stNnieCfg.aenNnieCoreId[0] = SVP_NNIE_ID_0; // set NNIE core

    /* Set process stop signal */
    g_stop_signal = HI_FALSE;

    /* Sys init */
    s32Ret = SAMPLE_COMM_SVP_CheckSysInit();
    SAMPLE_SVP_CHECK_EXPR_RET_VOID(s32Ret != HI_SUCCESS, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_COMM_SVP_CheckSysInit failed!\n");
    if (g_stop_signal == HI_TRUE) {
        SVP_NNIE_Yolov5_Lite_Stop();
        return;
    }

    /* Yolov5 Load model */
    MSG_DEBUG(DEBUG_LOG, "[INFO] : [SVP_NNIE_Yolov5_Lite] Load yolov5-lite model...\n");
    s32Ret = SAMPLE_COMM_SVP_NNIE_LoadModel(pcModelName, &s_stYolov5Model);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_COMM_SVP_NNIE_LoadModel failed!\n");

    /* Yolov5 parameter initialization */
    /* Yolov5 software parameters are set in SAMPLE_SVP_NNIE_Yolov5_SoftwareInit,
      if user has changed net struct, please make sure the parameter settings in
      SAMPLE_SVP_NNIE_Yolov5_SoftwareInit function are correct */
    MSG_DEBUG(DEBUG_LOG, "[INFO] : [SVP_NNIE_Yolov5_Lite] Initialize yolov5-lite parameters...\n");
    if (g_stop_signal == HI_TRUE) {
        SVP_NNIE_Yolov5_Lite_Stop();
        return;
    }
    s_stYolov5NnieParam.pstModel = &s_stYolov5Model.stModel;
    s32Ret = SVP_NNIE_Yolov5_ParamInit(&stNnieCfg, &s_stYolov5NnieParam);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SVP_NNIE_Yolov5_ParamInit failed!\n");
    
    /* record tskBuf */
    MSG_DEBUG(DEBUG_LOG, "[INFO] : [SVP_NNIE_Yolov5_Lite] NNIE addTskBuf...\n");
    s32Ret = HI_MPI_SVP_NNIE_AddTskBuf(&(s_stYolov5NnieParam.astForwardCtrl[0].stTskBuf)); // 记录 TskBuf 地址信息 ,用于减少内核态内存映射次数 ,提升效率 。
    SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret,YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_AddTskBuf failed!\n");

    /* Fill src data */
    SAMPLE_SVP_TRACE_INFO("Yolov5 start!\n");
    stInputDataIdx.u32SegIdx = 0;
    stInputDataIdx.u32NodeIdx = 0;
    if (g_stop_signal == HI_TRUE) {
        SVP_NNIE_Yolov5_Lite_Stop();
        return;
    }

    SAMPLE_SVP_TRACE_INFO("Load img!\n");
    s32Ret = SAMPLE_SVP_NNIE_FillSrcData(&stNnieCfg, &s_stYolov5NnieParam, &stInputDataIdx);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_SVP_NNIE_FillSrcData failed!\n");

    /* NNIE process(process the 0-th segment) */
    stProcSegIdx.u32SegIdx = 0;
    if (g_stop_signal == HI_TRUE) {
        SVP_NNIE_Yolov5_Lite_Stop();
        return;
    }

    SAMPLE_SVP_TRACE_INFO("NNIE forward!\n");
    s32Ret = SAMPLE_SVP_NNIE_Forward(&s_stYolov5NnieParam, &stInputDataIdx, &stProcSegIdx, HI_TRUE);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_1, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_SVP_NNIE_Forward failed!\n");

    /* Software process */
    /* if user has changed net struct, please make sure GetResult process function correct
     function input data are correct */
    if (g_stop_signal == HI_TRUE) {
        SVP_NNIE_Yolov5_Lite_Stop();
        return;
    }
    
    /* Get result from deal with output */
    SAMPLE_SVP_TRACE_INFO("Caculate output!\n");
    s32Ret = SVP_NNIE_Yolov5_GetResult(&s_stYolov5NnieParam, &output_node);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_1, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SVP_NNIE_Yolov5_GetResult failed!\n");

    SAMPLE_SVP_TRACE_INFO("SVP NNIE Yolov5 Postprocess!\n");
    /* Sort the result from list node and use nms get the bbox iou */
    if (output_node != NULL) {
        output_node = toSortList(output_node, NULL, SORT_SCORE_MODE, YOLO_MODEL_WIDTH / 2.);
        (void)SVP_NNIE_Yolov5_NonMaxSuppression(output_node);
    }

    /* Debug: printf the result list node */
    // (void)SAMPLE_SVP_NNIE_Detection_PrintResult(output_node);

    /* Release result */
    (void)SAMPLE_SVP_NNIE_Release_Result(output_node);

    /* print result, this sample has 81 classes:
     class 1:person          class 2:bicycle      class 3:car             class 4:motorbike      class 5:aeroplane 
     class 6:bus             class 7:train        class 8:truck           class 9:boat           class 10:traffic light 
     class 11:fire hydrant   class 12:stop sign   class 13:parking meter  class 14:bench         class 15:bird 
     class 16:cat            class 17:dog         class 18:horse          class 19:sheep         class 20:cow
     class 21:elephant       class 22:bear        class 23:zebra          class 24:giraffe       class 25:backpack
     class 26:umbrella       class 27:handbag     class 28:tie            class 29:suitcase      class 30:frisbee
     class 31:skis           class 32:snowboard   class 33:sports ball    class 34:kite          class 35:baseball bat
     class 36:baseball glove class 37:skateboard  class 38:surfboard      class 39:tennis racket class 40:bottle
     class 41:wine glass     class 42:cup         class 43:fork           class 44:knife         class 45:spoon
     class 46:bowl           class 47:banana      class 48:apple          class 49:sandwich      class 50orange
     class 51:broccoli       class 52:carrot      class 53:hot dog        class 54:pizza         class 55:donut
     class 56:cake           class 57:chair       class 58:sofa           class 59:pottedplant   class 60bed
     class 61:diningtable    class 62:toilet      class 63:vmonitor       class 64:laptop        class 65:mouse
     class 66:remote         class 67:keyboard    class 68:cell phone     class 69:microwave     class 70:oven
     class 71:toaster        class 72:sink        class 73:refrigerator   class 74:book          class 75:clock
     class 76:vase           class 77:scissors    class 78:teddy bear     class 79:hair drier    class 80:toothbrush */
YOLOV5_FAIL_1:
    /* Remove tskBuf */
    s32Ret = HI_MPI_SVP_NNIE_RemoveTskBuf(&(s_stYolov5NnieParam.astForwardCtrl[0].stTskBuf));
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_RemoveTskBuf failed!\n");

YOLOV5_FAIL_0:
    SVP_NNIE_Yolov5_Deinit(&s_stYolov5NnieParam, &s_stYolov5Model);
    SAMPLE_COMM_SVP_CheckSysExit();
}

/* function : unload model and params */
HI_S32 PauseDoUnloadModel( HI_VOID )
{
    HI_S32 s32Ret = HI_SUCCESS;

    /* Unload yolov5 model */
    SVP_NNIE_Yolov5_Lite_Stop();

    /* Unload hand detect model */

    return s32Ret;
}

/* function : Load ai model */
HI_S32 Load_Yolov5_Lite_Model( uintptr_t* model, const HI_CHAR *pcModelName )
{
    HI_S32 s32Ret = HI_SUCCESS;
    SAMPLE_SVP_NNIE_CFG_S *self = NULL;
    HI_U32 u32PicNum = 1;

    self = (SAMPLE_SVP_NNIE_CFG_S *)malloc(sizeof(*self));
    HI_ASSERT(self);
    memset_s(self, sizeof(*self), 0x00, sizeof(*self));

    /*
     * Set configuration parameter
     */
    self->pszPic = NULL;
    self->u32MaxInputNum = u32PicNum; // max input image num in each batch
    self->u32MaxRoiNum = 0;
    self->aenNnieCoreId[0] = SVP_NNIE_ID_0; // set NNIE core

    /* Yolov5 Load model */
    MSG_DEBUG(DEBUG_LOG, "[INFO] : [SVP_NNIE_Yolov5_Lite] Load yolov5-lite model...\n");
    s32Ret = SAMPLE_COMM_SVP_NNIE_LoadModel(pcModelName, &s_stYolov5Model);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_COMM_SVP_NNIE_LoadModel failed!\n");

    /* Yolov5 parameter initialization */
    /* Yolov5 software parameters are set in SAMPLE_SVP_NNIE_Yolov5_SoftwareInit,
      if user has changed net struct, please make sure the parameter settings in
      SAMPLE_SVP_NNIE_Yolov5_SoftwareInit function are correct */
    s_stYolov5NnieParam.pstModel = &s_stYolov5Model.stModel;
    s32Ret = SVP_NNIE_Yolov5_ParamInit(self, &s_stYolov5NnieParam);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_1, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SVP_NNIE_Yolov5_ParamInit failed!\n");

    /* record tskBuf */
    MSG_DEBUG(DEBUG_LOG, "[INFO] : [SVP_NNIE_Yolov5_Lite] NNIE addTskBuf...\n");
    s32Ret = HI_MPI_SVP_NNIE_AddTskBuf(&(s_stYolov5NnieParam.astForwardCtrl[0].stTskBuf)); // 记录 TskBuf 地址信息 ,用于减少内核态内存映射次数 ,提升效率 。
    SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret,YOLOV5_FAIL_1, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,HI_MPI_SVP_NNIE_AddTskBuf failed!\n");

    *model = (uintptr_t)self;
    return HI_SUCCESS;

    YOLOV5_FAIL_1:
        SVP_NNIE_Yolov5_Deinit(&s_stYolov5NnieParam, &s_stYolov5Model);
    YOLOV5_FAIL_0:
        *model = NULL;
        free(self);
        return HI_FAILURE;
}

/* function : fill yuv image data */
static HI_S32 FillNnieByImg(SAMPLE_SVP_NNIE_CFG_S* pstNnieCfg,
    SAMPLE_SVP_NNIE_PARAM_S *pstNnieParam, int segId, int nodeId, const IVE_IMAGE_S *img)
{
    HI_U32 i;
    HI_U32 j;
    HI_U32 n;
    HI_U32 u32Height = 0;
    HI_U32 u32Width = 0;
    HI_U32 u32Chn = 0;
    HI_U32 u32Stride = 0;
    HI_U32 u32VarSize;
    HI_U8 *pu8PicAddr = NULL;

    /*
     * 获取数据大小
     * Get data size
     */
    if (SVP_BLOB_TYPE_U8 <= pstNnieParam->astSegData[segId].astSrc[nodeId].enType &&
        SVP_BLOB_TYPE_YVU422SP >= pstNnieParam->astSegData[segId].astSrc[nodeId].enType) {
        u32VarSize = sizeof(HI_U8);
    } else {
        u32VarSize = sizeof(HI_U32);
    }

    /*
     * 填充源数据
     * Fill src data
     */
    if (SVP_BLOB_TYPE_SEQ_S32 == pstNnieParam->astSegData[segId].astSrc[nodeId].enType) {
        HI_ASSERT(0);
    } else {
        u32Height = pstNnieParam->astSegData[segId].astSrc[nodeId].unShape.stWhc.u32Height;
        u32Width = pstNnieParam->astSegData[segId].astSrc[nodeId].unShape.stWhc.u32Width;
        u32Chn = pstNnieParam->astSegData[segId].astSrc[nodeId].unShape.stWhc.u32Chn;
        u32Stride = pstNnieParam->astSegData[segId].astSrc[nodeId].u32Stride;
        pu8PicAddr = SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_U8,
            pstNnieParam->astSegData[segId].astSrc[nodeId].u64VirAddr);

        if (SVP_BLOB_TYPE_YVU420SP == pstNnieParam->astSegData[segId].astSrc[nodeId].enType) {
            HI_ASSERT(pstNnieParam->astSegData[segId].astSrc[nodeId].u32Num == 1);
            for (n = 0; n < pstNnieParam->astSegData[segId].astSrc[nodeId].u32Num; n++) {
                // Y
                const uint8_t *srcData = (const uint8_t*)(uintptr_t)img->au64VirAddr[0];
                HI_ASSERT(srcData);
                for (j = 0; j < u32Height; j++) {
                    if (memcpy_s(pu8PicAddr, u32Width * u32VarSize, srcData, u32Width * u32VarSize) != EOK) {
                        HI_ASSERT(0);
                    }
                    pu8PicAddr += u32Stride;
                    srcData += img->au32Stride[0];
                }
                // UV
                srcData = (const uint8_t*)(uintptr_t)img->au64VirAddr[1];
                HI_ASSERT(srcData);
                for (j = 0; j < u32Height / 2; j++) { // 2: 1/2Height
                    if (memcpy_s(pu8PicAddr, u32Width * u32VarSize, srcData, u32Width * u32VarSize) != EOK) {
                        HI_ASSERT(0);
                    }
                    pu8PicAddr += u32Stride;
                    srcData += img->au32Stride[1];
                }
            }
        } else if (SVP_BLOB_TYPE_YVU422SP == pstNnieParam->astSegData[segId].astSrc[nodeId].enType) {
            HI_ASSERT(0);
        } else {
            for (n = 0; n < pstNnieParam->astSegData[segId].astSrc[nodeId].u32Num; n++) {
                for (i = 0; i < u32Chn; i++) {
                    const uint8_t *srcData = (const uint8_t*)(uintptr_t)img->au64VirAddr[i];
                    HI_ASSERT(srcData);
                    for (j = 0; j < u32Height; j++) {
                        if (memcpy_s(pu8PicAddr, u32Width * u32VarSize, srcData, u32Width * u32VarSize) != EOK) {
                            HI_ASSERT(0);
                        }
                        pu8PicAddr += u32Stride;
                        srcData += img->au32Stride[i];
                    }
                }
            }
        }

        SAMPLE_COMM_SVP_FlushCache(pstNnieParam->astSegData[segId].astSrc[nodeId].u64PhyAddr,
            SAMPLE_SVP_NNIE_CONVERT_64BIT_ADDR(HI_VOID, pstNnieParam->astSegData[segId].astSrc[nodeId].u64VirAddr),
            pstNnieParam->astSegData[segId].astSrc[nodeId].u32Num*u32Chn*u32Height*u32Stride);
    }

    return HI_SUCCESS;
}

/* function : Calculation yuv image */
HI_S32 Yolo5CalImg(SAMPLE_SVP_NNIE_CFG_S* self, const IVE_IMAGE_S *img, SVP_NNIE_YOLOV5_BBOX_S **result_node)
{
    HI_S32 s32Ret;
    SAMPLE_SVP_NNIE_INPUT_DATA_INDEX_S stInputDataIdx = {0};
    SAMPLE_SVP_NNIE_PROCESS_SEG_INDEX_S stProcSegIdx = {0};
    SVP_NNIE_YOLOV5_BBOX_S *output_node = NULL;

    /* Fill src data */
    self->pszPic = NULL;
    stInputDataIdx.u32SegIdx = 0;
    stInputDataIdx.u32NodeIdx = 0;

    s32Ret = FillNnieByImg(self, &s_stYolov5NnieParam, 0, 0, img);
    SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,FillNnieByImg failed!\n");

    /* NNIE process(process the 0-th segment) */
    stProcSegIdx.u32SegIdx = 0;
    s32Ret = SAMPLE_SVP_NNIE_Forward(&s_stYolov5NnieParam, &stInputDataIdx, &stProcSegIdx, HI_TRUE);
    SAMPLE_SVP_CHECK_EXPR_GOTO(HI_SUCCESS != s32Ret, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SAMPLE_SVP_NNIE_Forward failed!\n");

    /* Get result from deal with output */
    s32Ret = SVP_NNIE_Yolov5_GetResult(&s_stYolov5NnieParam, &output_node);
    SAMPLE_SVP_CHECK_EXPR_GOTO(s32Ret != HI_SUCCESS, YOLOV5_FAIL_0, SAMPLE_SVP_ERR_LEVEL_ERROR,
        "Error,SVP_NNIE_Yolov5_GetResult failed!\n");

    /* Sort the result from list node and use nms get the bbox iou */
    if (output_node != NULL) {
        output_node = toSortList(output_node, NULL, SORT_SCORE_MODE, YOLO_MODEL_WIDTH / 2.);
        (void)SVP_NNIE_Yolov5_NonMaxSuppression(output_node);
        *result_node = output_node;
    }
    return HI_SUCCESS;

    YOLOV5_FAIL_0:
        return HI_FAILURE;
}

/* function : caculate the rect and center distance */
HI_S32 CaculateDiffCenter(RectBox bBox, HI_FLOAT cx)
{
    HI_DOUBLE res;
    res = abs(((bBox.xmin + bBox.xmax) / 2.) - cx);
    return res;
    // res = pow((bBox.xmin + bBox.xmax - YOLO_MODEL_WIDTH) / 2, 2) + pow((bBox.ymin + bBox.ymax - YOLO_MODEL_HEIGHT) / 2, 2);
    // return (HI_S32)sqrt(res);
}

static hi_u8 tracking_period = 0;   /* 0 for init period, 1 for tracking gesture, 2 for tracking turn face, 3 for tracking face */
/* function : confirm the tracking target and update the new tracking target */
HI_VOID UpdateTrackingTarget(SVP_NNIE_YOLOV5_BBOX_S* output, SAMPLE_IVE_KCF_S *pstIveKcfInfo)
{
    while (output != NULL)
    {
        /* If no tracking process now */
        if (pstIveKcfInfo->bUpdateDetect == HI_FALSE) return;
        
        /* near center and the class is person */
        if (output->u8ClassIdx == 0)
        {
            TrackFaceBoxs[TrackFaceObjNum] = output->bBox;
            TrackFaceObjNum++;
            if (TrackFaceObjNum >= SAMPLE_IVE_KCF_MAX_BBOX - 1) {
                break;
            }
        }
        if (output->u8ClassIdx == 3)
        {
            TrackGestureBoxs[TrackGestureObjNum] = output->bBox;
            TrackGestureObjNum++;
            if (TrackGestureObjNum >= SAMPLE_IVE_KCF_MAX_BBOX - 1) {
                break;
            }
        }

        output = output->next;
    }

    /* case for face */
    if (TrackFaceObjNum > 0)
    {
        /* select the nearest center rect */
        pstIveKcfInfo->trackRoi = TrackFaceBoxs[0];
        if (tracking_period == 2 || tracking_period == 3)
        {
            pstIveKcfInfo->bUpdateDetect = HI_FALSE;
            pstIveKcfInfo->enKcfProcStat = KCF_PROC_START;
        }
        TrackFaceObjNum = 0;
    }

    /* case for gesture */
    if (TrackGestureObjNum > 0)
    {
        pstIveKcfInfo->gestureRoi = TrackGestureBoxs[0];
        if (tracking_period == 1)
        {
            pstIveKcfInfo->bUpdateDetect = HI_FALSE;
            pstIveKcfInfo->enKcfProcStat = KCF_PROC_START;
        }
        TrackGestureObjNum = 0;
    }
}

/* function : deal with the gesture result output */
HI_VOID SAMPLE_SVP_NNIE_DealWithResult(SVP_NNIE_YOLOV5_BBOX_S *res, hi_u8 *cfm_class, hi_float *cx)
{
    HI_U8 gesture_nums = 0;
    static HI_U8 last_ges_nums = 0;
    
    HI_U8 gesture_class = 0;
    HI_FLOAT box_cx = 160.;
    static HI_U8 last_gesture = 0;
    
    static HI_U8 confirm_times = 0;
    while (res != NULL)
    {
        if (res->u8ClassIdx > 0)
        {
            if (gesture_class == 0)
            {
                gesture_class = res->u8ClassIdx;
                box_cx = (res->bBox.xmin + res->bBox.xmax) / 2.;
            }
            gesture_nums++;
        }
        res = res->next;
    }

    if (gesture_nums == 0 || last_ges_nums != gesture_nums)
    {
        *cfm_class = 0;
        *cx = 160.;
        confirm_times = 0;
        last_ges_nums = gesture_nums;
        return;
    }

    if (last_gesture == gesture_class)
    {
        confirm_times++;
    } else {
        confirm_times = 0;
    }

    /* confirm n times to judge the gesture class */
    if (confirm_times >= 5)
    {
        *cfm_class = gesture_class;
        *cx = box_cx;
    }
    last_gesture = gesture_class;
}

/* function : caculate the iou between two rect */
static HI_FLOAT cal_roi_from_bbox(RectBox box1, RectBox box2)
{
    HI_FLOAT iou1, iou2;

    /* Caculate two iou area */
    iou1 = (box1.xmax - box1.xmin + 1.) * (box1.ymax - box1.ymin + 1.);
    iou2 = (box2.xmax - box2.xmin + 1.) * (box2.ymax - box2.ymin + 1.);

    /* Caculate the interiou */
    HI_FLOAT interIou;

    HI_FLOAT xmin = fmax(box1.xmin, box2.xmin);
    HI_FLOAT ymin = fmax(box1.ymin, box2.ymin);
    HI_FLOAT xmax = fmin(box1.xmax, box2.xmax);
    HI_FLOAT ymax = fmin(box1.ymax, box2.ymax);

    HI_FLOAT inter_h = fmax(ymax - ymin + 1., 0.);
    HI_FLOAT inter_w = fmax(xmax - xmin + 1., 0.);

    interIou = inter_h * inter_w;

    /* Caculate the iou ratio */
    return (HI_FLOAT)(interIou / (iou1 + iou2 - interIou));
}

/* function : Draw_Lines_Mode */
static HI_S32 Draw_Lines_Mode(VIDEO_FRAME_INFO_S *frm, hi_u8 mode)
{
    VGS_DRAW_LINE_S lines[5];
    int lineNum = 0;
    switch (mode)
    {
        case 0:
        {
            lines[0].stStartPoint.s32X = 1720;
            lines[0].stStartPoint.s32Y = 150;
            lines[0].stEndPoint.s32X = 1720;
            lines[0].stEndPoint.s32Y = 150 + 50;
            lines[0].u32Color = RGB888_GREEN;
            lines[0].u32Thick = DRAW_RETC_THICK;
            lines[1].stStartPoint.s32X = 1720;
            lines[1].stStartPoint.s32Y = 150;
            lines[1].stEndPoint.s32X = 1720 + 30;
            lines[1].stEndPoint.s32Y = 150;
            lines[1].u32Color = RGB888_GREEN;
            lines[1].u32Thick = DRAW_RETC_THICK;
            lines[2].stStartPoint.s32X = 1720 + 30;
            lines[2].stStartPoint.s32Y = 150;
            lines[2].stEndPoint.s32X = 1720 + 30;
            lines[2].stEndPoint.s32Y = 150 + 50;
            lines[2].u32Color = RGB888_GREEN;
            lines[2].u32Thick = DRAW_RETC_THICK;
            lines[3].stStartPoint.s32X = 1720;
            lines[3].stStartPoint.s32Y = 150 + 50;
            lines[3].stEndPoint.s32X = 1720 + 30;
            lines[3].stEndPoint.s32Y = 150 + 50;
            lines[3].u32Color = RGB888_GREEN;
            lines[3].u32Thick = DRAW_RETC_THICK;
            lineNum = 4;
            break;
        }
        case 1:
        {
            lines[0].stStartPoint.s32X = 1720 + 30;
            lines[0].stStartPoint.s32Y = 150;
            lines[0].stEndPoint.s32X = 1720 + 30;
            lines[0].stEndPoint.s32Y = 150 + 50;
            lines[0].u32Color = RGB888_GREEN;
            lines[0].u32Thick = DRAW_RETC_THICK;
            lineNum = 1;
            break;
        }
        case 2:
        {
            lines[0].stStartPoint.s32X = 1720;
            lines[0].stStartPoint.s32Y = 150;
            lines[0].stEndPoint.s32X = 1720 + 30;
            lines[0].stEndPoint.s32Y = 150;
            lines[0].u32Color = RGB888_BLUE;
            lines[0].u32Thick = DRAW_RETC_THICK;
            lines[1].stStartPoint.s32X = 1720 + 30;
            lines[1].stStartPoint.s32Y = 150;
            lines[1].stEndPoint.s32X = 1720 + 30;
            lines[1].stEndPoint.s32Y = 150 + 26;
            lines[1].u32Color = RGB888_BLUE;
            lines[1].u32Thick = DRAW_RETC_THICK;
            lines[2].stStartPoint.s32X = 1720;
            lines[2].stStartPoint.s32Y = 150 + 26;
            lines[2].stEndPoint.s32X = 1720 + 30;
            lines[2].stEndPoint.s32Y = 150 + 26;
            lines[2].u32Color = RGB888_BLUE;
            lines[2].u32Thick = DRAW_RETC_THICK;
            lines[3].stStartPoint.s32X = 1720;
            lines[3].stStartPoint.s32Y = 150 + 26;
            lines[3].stEndPoint.s32X = 1720;
            lines[3].stEndPoint.s32Y = 150 + 52;
            lines[3].u32Color = RGB888_BLUE;
            lines[3].u32Thick = DRAW_RETC_THICK;
            lines[4].stStartPoint.s32X = 1720;
            lines[4].stStartPoint.s32Y = 150 + 52;
            lines[4].stEndPoint.s32X = 1720 + 30;
            lines[4].stEndPoint.s32Y = 150 + 52;
            lines[4].u32Color = RGB888_BLUE;
            lines[4].u32Thick = DRAW_RETC_THICK;
            lineNum = 5;
            break;
        }
        case 3:
        {
            lines[0].stStartPoint.s32X = 1720;
            lines[0].stStartPoint.s32Y = 150;
            lines[0].stEndPoint.s32X = 1720 + 30;
            lines[0].stEndPoint.s32Y = 150;
            lines[0].u32Color = RGB888_RED;
            lines[0].u32Thick = DRAW_RETC_THICK;
            lines[1].stStartPoint.s32X = 1720 + 30;
            lines[1].stStartPoint.s32Y = 150;
            lines[1].stEndPoint.s32X = 1720 + 30;
            lines[1].stEndPoint.s32Y = 150 + 52;
            lines[1].u32Color = RGB888_RED;
            lines[1].u32Thick = DRAW_RETC_THICK;
            lines[2].stStartPoint.s32X = 1720;
            lines[2].stStartPoint.s32Y = 150 + 26;
            lines[2].stEndPoint.s32X = 1720 + 30;
            lines[2].stEndPoint.s32Y = 150 + 26;
            lines[2].u32Color = RGB888_RED;
            lines[2].u32Thick = DRAW_RETC_THICK;
            lines[3].stStartPoint.s32X = 1720;
            lines[3].stStartPoint.s32Y = 150 + 52;
            lines[3].stEndPoint.s32X = 1720 + 30;
            lines[3].stEndPoint.s32Y = 150 + 52;
            lines[3].u32Color = RGB888_RED;
            lines[3].u32Thick = DRAW_RETC_THICK;
            lineNum = 4;
            break;
        }
        default:
            break;
    }
    return VgsDrawLines(frm, lines, lineNum);
}

static hi_u8 cfm_count = 0;
static SAMPLE_IVE_LBPH_MATCH_PROC_STATUS_E lbph_proc = LBPH_PROC_BUTT;
static hi_u8 line_mode = 0;

/* function : yolov5 detection and gesture calculation */
HI_S32 Yolov5DetectResnetClassifyCal(uintptr_t model, VIDEO_FRAME_INFO_S *srcFrm, VIDEO_FRAME_INFO_S *dstFrm, SAMPLE_IVE_KCF_S *pstIveKcfInfo)
{
    HI_S32 s32Ret;
    SAMPLE_SVP_NNIE_CFG_S *self = (SAMPLE_SVP_NNIE_CFG_S*)model;
    SVP_NNIE_YOLOV5_BBOX_S *output_node = NULL;

    HI_S32 uart_sendLen = 0;
    HI_CHAR sendMsgBuf[HISIGNALLING_MSG_ONE_FRAME_LEN] = { 0 };

    hi_u8 gesture_class = 0;
    hi_float gesture_cx = 160.;

    static RectBox last_bbox = { 0 };
    static hi_float last_score = 0;
    static RectBox valid_bbox = { 0 };
    static RectBox matchBox = { 0 };
    static HI_FLOAT match_score = 0;
    static hi_u8 update_count = 0;
    static HI_BOOL match_isValid = HI_FALSE;
    static HI_BOOL isNewTemp = HI_FALSE;

    /* transform the yuv to img */
    s32Ret = FrmToOrigImg((VIDEO_FRAME_INFO_S*)srcFrm, &img);
    SAMPLE_CHECK_EXPR_RET(s32Ret != HI_SUCCESS, s32Ret, "hand detect for YUV Frm to Img FAIL, s32Ret=%#x\n", s32Ret);

    /* post process and nms the frame */
    s32Ret = Yolo5CalImg(self, &img, &output_node);

    /* Debug: printf the result list node */
    // (void)SAMPLE_SVP_NNIE_Detection_PrintResult(output_node);

    /* 
     * 对目标识别结果进行排序（按靠近中心点进行排序）
     * 先寻找需要跟踪的目标 后续通过特征匹配对跟踪目标进行纠正
     * Sort the target recognition results (sort by proximity to the center point)
     * First, locate the target that needs to be tracked, and then correct the tracking target through feature matching.
     */
    output_node = toSortList(output_node, NULL, SORT_CENTER_MODE, YOLO_MODEL_WIDTH / 2.);

    /* Update Tracking target */
    UpdateTrackingTarget(output_node, pstIveKcfInfo);
    
    /* Debug : draw the select target */
    /* If find the tracking target, use kcf algorithm */
    if (pstIveKcfInfo->enKcfProcStat == KCF_PROC_START || pstIveKcfInfo->enKcfProcStat == KCF_PROC_PROC) {
        RectBox trackBox;
        HI_BOOL isMatch = HI_FALSE;
        // SAMPLE_SVP_NNIE_DrawTrackingRoi(pstIveKcfInfo, dstFrm);
        /* case 1: if tracking gesture */
        if (tracking_period == 1)
        {
            trackBox = pstIveKcfInfo->gestureRoi;
            pstIveKcfInfo->bUpdateDetect = HI_TRUE;
            isMatch = HI_TRUE;
            if (fabs((pstIveKcfInfo->gestureRoi.xmax + pstIveKcfInfo->gestureRoi.xmin) / 2 - 160) < 5)
            {
                /* turn to case 2: gesture and face */
                tracking_period = 2;
            }    
        }
        /* case 2: turn gesture to face or tracking face */
        if (tracking_period == 2 || tracking_period == 3)
        {
            if (fabs((pstIveKcfInfo->trackRoi.xmax + pstIveKcfInfo->trackRoi.xmin) / 2 - 160) < 5) {
                tracking_period = 3;
                pstIveKcfInfo->bUpdateDetect = HI_FALSE;
            }
            if (tracking_period == 2)
            {
                trackBox = pstIveKcfInfo->trackRoi;
                pstIveKcfInfo->bUpdateDetect = HI_TRUE;
                isMatch = HI_TRUE;
                lbph_proc = LBPH_PROC_END;
            }
            /* case 3: tracking face */
            if (tracking_period == 3)
            {
                trackBox = tracker.KalmanTrackingTarget(pstIveKcfInfo, output_node, 0.05, &isMatch);

                if (match_isValid)
                {
                    if (cal_roi_from_bbox(trackBox, valid_bbox) <= 0.05)
                    {
                        pstIveKcfInfo->trackRoi = valid_bbox;
                        pstIveKcfInfo->enKcfProcStat = KCF_PROC_START;
                        printf("update kcf track bbox\n");
                    }
                    match_isValid = HI_FALSE;
                }
            }
        }

        uart_sendLen = 0;
        HI_U16 box_cx = (trackBox.xmin + trackBox.xmax) * 100.0 / 2 ;
        HI_U16 box_cy = (trackBox.ymin + trackBox.ymax) * 100.0 / 2 ;
        // if (tracking_period == 1 || tracking_period == 2)
        {
            box_cx += (16000 - box_cx) * 0.4;
            box_cy += (14000 - box_cy) * 0.4;
        }
        sendMsgBuf[uart_sendLen++] = isMatch;
        sendMsgBuf[uart_sendLen++] = (HI_UCHAR)((box_cx & 0xFF00) >> 8);
        sendMsgBuf[uart_sendLen++] = (HI_UCHAR)(box_cx & 0x00FF);
        sendMsgBuf[uart_sendLen++] = (HI_UCHAR)((box_cy & 0xFF00) >> 8);
        sendMsgBuf[uart_sendLen++] = (HI_UCHAR)(box_cy & 0x00FF);
        HisignallingMsgSend(uartFd, sendMsgBuf, uart_sendLen, TargetRoiPosi);

        // printf("INFO : [Yolov5DetectResnetClassifyCal] cx %d, cy %d\n", box_cx, box_cy);

        RectBoxTran(&trackBox, YOLO_MODEL_WIDTH, YOLO_MODEL_HEIGHT, dstFrm->stVFrame.u32Width, dstFrm->stVFrame.u32Height);
        s32Ret = MppFrmDrawRects(dstFrm, &trackBox, 1, RGB888_GREEN, DRAW_RETC_THICK);
    }

    /* Deal with the detection result. */
    (void)SAMPLE_SVP_NNIE_DealWithResult(output_node, &gesture_class, &gesture_cx);

    /* 
     * 开启目标跟踪后进行匹配模板的初始化
     * 或者
     * 已经初始化过匹配模板，周期对匹配模板进行更新
     * Initialize the matching template after target tracking is enabled
     * or
     * The matching template has been initialized and updated periodically
     */
    if (tracking_period == 3)
    {
        /* use gesture 2 to update new face */
        if (gesture_class == 3)
        {
            output_node = toSortList(output_node, NULL, SORT_CENTER_MODE, gesture_cx);
            lbph_proc = LBPH_PROC_START;
            update_count = 0;
            match_isValid = HI_FALSE;
            line_mode = 3;
        }
        
        switch (lbph_proc)
        {
            /* start! init face tempalte */
            case LBPH_PROC_START:
                IVE_LBPH_READY_FOR_TASK(srcFrm, output_node, HI_TRUE);
                if (IVE_LBPH_RETURN_RESULT(&matchBox, &match_score, &isNewTemp))
                {
                    if (isNewTemp == HI_TRUE) lbph_proc = LBPH_PROC_INITED;
                    if (line_mode == 3) line_mode = 1;
                }
                break;
            
            case LBPH_PROC_INITED:
                IVE_LBPH_READY_FOR_TASK(srcFrm, output_node, HI_FALSE);
                if (IVE_LBPH_RETURN_RESULT(&matchBox, &match_score, &isNewTemp))
                {
                    if (isNewTemp == HI_FALSE)
                    {
                        if (cal_roi_from_bbox(last_bbox, matchBox) >= 0.6)
                        {
                            update_count++;
                        } else {
                            update_count = 0;
                        }
                        
                        if (update_count >= 3)
                        {
                            update_count = 0;
                            match_isValid = HI_TRUE;
                            valid_bbox = matchBox;
                            printf("update match temp\n");
                        }

                        last_bbox = matchBox;
                        last_score = match_score;
                        
                        RectBoxTran(&matchBox, YOLO_MODEL_WIDTH, YOLO_MODEL_HEIGHT, dstFrm->stVFrame.u32Width, dstFrm->stVFrame.u32Height);
                        s32Ret = MppFrmDrawRects(dstFrm, &matchBox, 1, RGB888_BLUE, DRAW_RETC_THICK);
                        printf("score: %.2f\n", match_score);
                    }
                }
                break;

            case LBPH_PROC_END:
                lbph_proc = LBPH_PROC_START;
                break;
                
            default:
                break;
        }
    }

    /* case 1: if tracking gesture, no gesture, turn to tracking face */
    if (tracking_period == 1 && gesture_class != 3)
    {
        cfm_count++;
        if (cfm_count >= 10) {
            tracking_period = 2;
        }
    } else {
        cfm_count = 0;
    }

    if (gesture_class == 3 && pstIveKcfInfo->enKcfProcStat == KCF_PROC_END) {
        // printf("INFO : [Yolov5DetectResnetClassifyCal] Kalman tracking algorithm start.\n");
        HisignallingMsgSend(uartFd, (char *)&gesture_class, 1, GestureClass);
        /* period for tracking gesture. */
        tracking_period = 1;
        line_mode = 1;
    } else if (gesture_class == 6) {
        // printf("INFO : [Yolov5DetectResnetClassifyCal] Kalman tracking algorithm stop.\n");
        if (pstIveKcfInfo->enKcfProcStat != KCF_PROC_END) {
            pstIveKcfInfo->enKcfProcStat = KCF_PROC_END;
            pstIveKcfInfo->bUpdateDetect = HI_FALSE;
            lbph_proc = LBPH_PROC_END;
        }
        HisignallingMsgSend(uartFd, (char *)&gesture_class, 1, GestureClass);
        /* init tracking period */
        tracking_period = 0;
        line_mode = 0;
    } else if (gesture_class == 2 || gesture_class == 5) {
        HisignallingMsgSend(uartFd, (char *)&gesture_class, 1, GestureClass);
        line_mode = 2;
    }

    /* Debug : Draw rect img */
    s32Ret = SAMPLE_SVP_NNIE_DrawResultRect(output_node, dstFrm);
    s32Ret = Draw_Lines_Mode(dstFrm, line_mode);

    /* Release result */
    (void)SAMPLE_SVP_NNIE_Release_Result(output_node);

    return s32Ret;
}

HI_VOID ParseUartMsgProcess(HI_UCHAR *msg, SAMPLE_IVE_KCF_S *pstIveKcfInfo)
{
    HI_U8 payload_idx = 0;
    /* 0xAA 0x55 type datalen data crc1 crc2 */
    if (msg[payload_idx] != HISIGNALLING_HEAD_1 && msg[payload_idx + 1] != HISIGNALLING_HEAD_2) {
        printf("ERROR: [ParseUartMsgProcess] frame header err.\n");
        return;
    }

    unsigned short crc = 0;
    HI_U8 msg_len = msg[3];
    crc = crc16(msg, msg_len + HISIGNALLING_MSG_HEADER_TAIL_LEN);
    
    if ((crc & 0xFFFF) >> 8 != msg[msg_len + HISIGNALLING_MSG_HEADER_TAIL_LEN] &&
        crc & 0x00FF != msg[msg_len + HISIGNALLING_MSG_HEADER_TAIL_LEN + 1])
    {
        printf("ERROR: [ParseUartMsgProcess] crc err.\n");
    }

    HI_U8 cmd = msg[2];
    switch (cmd)
    {
        case isTrackingStart:
            /* start new target tracking process */
            if (msg[4] == 1 && pstIveKcfInfo->enKcfProcStat == KCF_PROC_END) {
                pstIveKcfInfo->bUpdateDetect = HI_TRUE;
                if (tracking_period != 1) tracking_period = 2;
                printf("INFO: [ParseUartMsgProcess] start kalman tracking.\n");
            }
            /* stop the tracking process */
            if (msg[4] == 0)
            {
                pstIveKcfInfo->enKcfProcStat = KCF_PROC_END;
                pstIveKcfInfo->bUpdateDetect = HI_FALSE;
                tracking_period = 0;
                printf("INFO : [ParseUartMsgProcess] stop kalman trackin.\n");
            }
            break;
        default:
            break;
    }
}
